﻿using System;
using System.Collections.Generic;
using System.Text;
using XDL.Framework;
using XDL.Framework.Collections;

namespace XDL.Framework.OpenSteer
{
        /// <summary>
        /// A-Star path finding algorithm.
        /// </summary>
        /// <remarks>
        /// Based on code sample from:
        /// Bidou (http://www.csharpfr.com/auteurdetail.aspx?ID=13319)
        /// Blog   : http://blogs.developpeur.org/bidou/
        /// Date   : January 2007
        /// </remarks>
    public class AStarPath
    {
        const int OpenSet = 0;
        const int ClosedSet = 1;
        Vector2i m_startPoint;
        Vector2i m_endPoint;
        IMap m_map = null;
        PriorityQueueB<PathNode> m_open = new PriorityQueueB<PathNode>(new PathNode.Comparer());

        /// ----------------------------------------------------------------------------------------
        /// <summary>
        /// Create a new AStar object.
        /// </summary>
        /// <param name="map"> The map. </param>
        /// ----------------------------------------------------------------------------------------
        public AStarPath(IMap map)
        {
            if (map == null)
                throw new ArgumentException("map cannot be null");
            m_map = map;
        }

        public IMap Map
        {
            get
            {
                return m_map;
            }
        }

        public Vector2i StartPoint
        {
            get
            {
                return m_startPoint;
            }
        }

        public Vector2i EndPoint
        {
            get
            {
                return m_endPoint;
            }
        }

        int m_currentTag = 0;
        // m_close.Clear();
        PathNode[] m_neighbors = new PathNode[PathNode.Directions];

        /// <summary>
        /// Calculate the shortest path between the start point and the end point.
        /// </summary>
        /// <remarks>The path is reversed. The algorithm is <b>not</b> thread safe.</remarks>
        /// <returns>The shortest path.</returns>
        public List<Vector2i> GetPath(Vector2i startPoint, Vector2i endPoint)
        {
            // update targets
            m_startPoint = startPoint;
            m_endPoint = endPoint;
            m_currentTag++;
            m_open.Clear();

            // Set the resolution "context"
            PathNode.ResolutionContext = this;
            PathNode startNode = new PathNode(null, m_startPoint);
            m_open.Push(startNode);
            Map.Tag(startNode.Position, m_currentTag, OpenSet); // m_open.Add(startNode);            

            while (m_open.Count > 0)
            {
                // This is the best node
                PathNode best = m_open.Pop();
                Map.Tag(startNode.Position, -1, OpenSet);

                // We are finished
                if (best.Position == m_endPoint)
                    return ReconstructPath(best);
                // mark the node as visited
                Map.Tag(best.Position, m_currentTag, ClosedSet); // m_close.Add(best);                
                // get valid neighbors
                int neighborsCount = best.GetNeighbors(m_neighbors);
                if (neighborsCount > 0)
                    AddToOpen(best, m_neighbors, neighborsCount);
            }
            // 
            PathNode.ResolutionContext = null;

            // No path found
            return null;
        }


        List<Vector2i> ReconstructPath(PathNode node)
        {
            // reconstruct the path until we find the starting point
            List<Vector2i> sol = new List<Vector2i>();
            while (node.Parent != null)
            {
                sol.Add(node.Position);
                // go backward
                node = node.Parent;
            }
            return sol; // Return the solution when the parent is null (the first point)
        }

        /// <summary>
        /// Add a list of nodes to the open list if needed.
        /// </summary>
        /// <param name="current"> The current nodes. </param>
        /// <param name="nodes"> The nodes to add. </param>
        void AddToOpen(PathNode current, PathNode[] nodes, int count)
        {
            for (int i = 0; i < count; i++)
            {
                PathNode node = nodes[i];
                if (!Map.IsTagged(node.Position, m_currentTag, OpenSet)) // if (!m_open.Contains(node))
                {
                    if (!Map.IsTagged(node.Position, m_currentTag, ClosedSet)) // if (!m_close.Contains(node))    
                    {
                        m_open.Push(node);
                        Map.Tag(node.Position, m_currentTag, OpenSet); // m_open.AddDichotomic(node);
                    }
                }
                // Else really nedded ?
                else
                {
                    if (node.CostWillBe() < node.Cost)
                        node.Parent = current;
                }
            }
        }
    }
}
